#
# Copyright (C) 2023, Inria
# GRAPHDECO research group, https://team.inria.fr/graphdeco
# All rights reserved.
#
# This software is free for non-commercial, research and evaluation use 
# under the terms of the LICENSE.md file.
#
# For inquiries contact  george.drettakis@inria.fr
#

import torch
import numpy as np
from utils.general_utils import inverse_sigmoid, get_expon_lr_func, build_rotation
from torch import nn
import os, math
import torch.nn.functional as F
from utils.system_utils import mkdir_p
from plyfile import PlyData, PlyElement
from utils.sh_utils import RGB2SH
from simple_knn._C import distCUDA2
from .cameras import Camera
from utils.graphics_utils import BasicPointCloud, compute_camera_distance_by_RT, compute_camera_distance_by_unproject
from utils.general_utils import strip_symmetric, build_scaling_rotation
from arguments import ModelParams, OptimizationParams
from scene.embedding import MLP


class GaussianModel:

    def setup_functions(self):
        def build_covariance_from_scaling_rotation(scaling, scaling_modifier, rotation):
            L = build_scaling_rotation(scaling_modifier * scaling, rotation)
            actual_covariance = L @ L.transpose(1, 2)
            symm = strip_symmetric(actual_covariance)
            return symm
        
        self.scaling_activation = torch.exp
        self.scaling_inverse_activation = torch.log
        self.covariance_activation = build_covariance_from_scaling_rotation
        self.opacity_activation = torch.sigmoid
        self.inverse_opacity_activation = inverse_sigmoid
        self.rotation_activation = torch.nn.functional.normalize


    def __init__(self, config: ModelParams, args: OptimizationParams):
        self.args = args
        self.config = config
        self.active_sh_degree = 0        
        self.max_sh_degree = config.sh_degree   
        
        self._xyz = torch.empty(0)     
        self._shs_dc = torch.empty(0)  
        self._shs_rest = torch.empty(0) 
        self._scaling = torch.empty(0)    
        self._rotation = torch.empty(0) 
        self._opacity = torch.empty(0)
        self._alpha = torch.empty(0)
        self._offsets = torch.empty(0)
        self._cholesky= torch.empty(0)
        
        self.chol_min = torch.tensor([0.0000, -0.8600,  0.0000]).cuda()
        self.chol_max = torch.tensor([2.3600, 1.5400, 2.2300]).cuda()
        
        self.max_radii2D = torch.empty(0) 
        self.xyz_gradient_accum = torch.empty(0)
        self.denom = torch.empty(0)
        self.optimizer = None
        self.percent_dense = 0
        self.spatial_lr_scale = 0
        self.setup_functions()
        
        # setting 2D gaussian attributes decoders
        self.add_plucker = config.add_plucker
        self.feat_dim = config.feat_dim
        self.hid_dim = config.hid_dim
        self.n_layers = config.n_layers
        self.n_offsets = config.n_offsets
        
        feat_in_dim = (self.feat_dim+6+3) if self.add_plucker else self.feat_dim+3
        self.mlp_opacity     = MLP(feat_in_dim, self.hid_dim, self.n_offsets*1, n_layers=self.n_layers, out_act=nn.Tanh()).cuda()
        self.mlp_cholesky    = MLP(feat_in_dim, self.hid_dim, self.n_offsets*3, n_layers=self.n_layers, out_act=nn.Sigmoid()).cuda()
        self.mlp_color       = MLP(feat_in_dim, self.hid_dim, self.n_offsets*3, n_layers=self.n_layers, out_act=nn.Sigmoid()).cuda()
        self.mlp_offset      = MLP(feat_in_dim, self.hid_dim, self.n_offsets*2, n_layers=self.n_layers, out_act=nn.Tanh()).cuda()
            
            
    def capture(self):
        return (
            self.active_sh_degree,
            self._xyz,
            self._shs_dc,
            self._shs_rest,
            self._scaling,
            self._rotation,
            self._opacity,
            self._feats,
            self.max_radii2D,
            self.xyz_gradient_accum,
            self.denom,
            self.optimizer.state_dict(),
            self.spatial_lr_scale,
        )
    
    def restore(self, model_args, training_args):
        (self.active_sh_degree, 
        self._xyz, 
        self._shs_dc, 
        self._features_rest,
        self._scaling, 
        self._rotation, 
        self._opacity,
        self._feats,
        self.max_radii2D, 
        xyz_gradient_accum, 
        denom,
        opt_dict, 
        self.spatial_lr_scale) = model_args
        self.training_setup(training_args)
        self.xyz_gradient_accum = xyz_gradient_accum
        self.denom = denom
        self.optimizer.load_state_dict(opt_dict)

    @property
    def get_scaling(self):
        return self.scaling_activation(self._scaling)

    @property
    def get_feats(self):
        return self._feats
    
    @property
    def get_rotation(self):
        return self.rotation_activation(self._rotation)
    
    @property
    def get_xyz(self):
        return self._xyz
    
    @property
    def get_shs(self):
        shs_dc = self._shs_dc
        shs_rest = self._shs_rest
        return torch.cat((shs_dc, shs_rest), dim=1)
    
    @property
    def get_opacity(self):
        return self.opacity_activation(self._opacity)
    
    @property
    def get_offset(self):
        return torch.tanh(self._offsets)
    
    @property
    def get_alpha(self):
        return torch.sigmoid(self._alpha)
    
    @property
    def get_cholesky(self):
        return torch.sigmoid(self._cholesky)
    
    @property
    def get_cholesky_mlp(self):
        return self.mlp_cholesky
    
    @property
    def get_opacity_mlp(self):
        return self.mlp_opacity
    
    @property
    def get_color_mlp(self):
        return self.mlp_color
    
    @property
    def get_offset_mlp(self):
        return self.mlp_offset
    
    def get_covariance(self, scaling_modifier=1):
        return self.covariance_activation(self.get_scaling, scaling_modifier, self._rotation)

    def oneupSHdegree(self):
        if self.active_sh_degree < self.max_sh_degree:
            self.active_sh_degree += 1

    def create_from_pcd(self, pcd : BasicPointCloud, spatial_lr_scale : float):
        self.spatial_lr_scale = spatial_lr_scale
        
        fused_point_cloud = torch.tensor(np.asarray(pcd.points)).float().cuda()
        fused_color = RGB2SH(torch.tensor(np.asarray(pcd.colors)).float().cuda())
        shs = torch.zeros((fused_color.shape[0], 3, (self.max_sh_degree + 1) ** 2)).float().cuda()
        shs[:, :3, 0 ] = fused_color
        shs[:, 3:, 1:] = 0.0

        print("Number of points at initialisation : ", fused_point_cloud.shape[0])

        dist2 = torch.clamp_min(distCUDA2(torch.from_numpy(np.asarray(pcd.points)).float().cuda()), 0.0000001)
        scales = torch.log(torch.sqrt(dist2))[...,None].repeat(1, 3)        # (N, 3)
        rots = torch.zeros((fused_point_cloud.shape[0], 4), device="cuda")  # (N, 4)
        rots[:, 0] = 1

        opacities = inverse_sigmoid(0.1 * torch.ones((fused_point_cloud.shape[0], 1), dtype=torch.float, device="cuda"))
        alpha = torch.randn((fused_point_cloud.shape[0], 1)).float().cuda()
        offsets = torch.randn((fused_point_cloud.shape[0], 2)).float().cuda()
        cholesky = torch.randn((fused_point_cloud.shape[0], 3)).float().cuda()
        features = torch.randn((fused_point_cloud.shape[0], self.feat_dim)).float().cuda()
        
        self._xyz = nn.Parameter(fused_point_cloud.requires_grad_(True))                                        # (N, 3)
        self._shs_dc = nn.Parameter(shs[:,:,0:1].transpose(1, 2).contiguous().requires_grad_(True))             # (N, 1, 3)
        self._shs_rest = nn.Parameter(shs[:,:,1:].transpose(1, 2).contiguous().requires_grad_(True))            # (N, 15, 3)
        self._scaling = nn.Parameter(scales.requires_grad_(True))                                               # (N, 3)
        self._rotation = nn.Parameter(rots.requires_grad_(True))                                                # (N, 4)
        self._opacity = nn.Parameter(opacities.requires_grad_(True))                                            # (N, 1)
        self._alpha = nn.Parameter(alpha.requires_grad_(True))                                                  # (N, 1)
        self._offsets = nn.Parameter(offsets.requires_grad_(True))
        self._cholesky = nn.Parameter(cholesky.requires_grad_(True))
        self._feats = nn.Parameter(features.requires_grad_(True))
        self.max_radii2D = torch.zeros((self.get_xyz.shape[0]), device="cuda")                                  # (N, )

    def training_setup(self, training_args: OptimizationParams):
        self.percent_dense = training_args.percent_dense # 0.01
        self.xyz_gradient_accum = torch.zeros((self.get_xyz.shape[0], 1), device="cuda")    # (N, 1)
        self.denom = torch.zeros((self.get_xyz.shape[0], 1), device="cuda")                 # (N, 1)

        l = [
            {'params': [self._xyz], 'lr': training_args.position_lr_init * self.spatial_lr_scale, "name": "xyz"},
            {'params': [self._shs_dc], 'lr': training_args.shs_lr, "name": "shs_dc"},
            {'params': [self._shs_rest], 'lr': training_args.shs_lr / 20.0, "name": "shs_rest"},
            {'params': [self._opacity], 'lr': training_args.opacity_lr, "name": "opacity"},
            {'params': [self._scaling], 'lr': training_args.scaling_lr, "name": "scaling"},
            {'params': [self._rotation], 'lr': training_args.rotation_lr, "name": "rotation"},
            
            {'params': [self._alpha], 'lr': training_args.alpha_lr, "name": "alpha"},
            {'params': [self._offsets], 'lr': training_args.offsets_lr, "name": "offsets"},
            {'params': [self._cholesky], 'lr': training_args.cholesky_lr, "name": "cholesky"},
            {'params': [self._feats], 'lr': training_args.cholesky_lr, "name": "feats"},
            
            {'params': self.mlp_opacity.parameters(), 'lr': training_args.mlp_opacity_lr_init, "name": "mlp_opacity"},
            {'params': self.mlp_color.parameters(), 'lr': training_args.mlp_color_lr_init, "name": "mlp_color"},
            {'params': self.mlp_cholesky.parameters(), 'lr': training_args.mlp_cholesky_lr_init, "name": "mlp_cholesky"},
            {'params': self.mlp_offset.parameters(), 'lr': training_args.mlp_offset_lr_init, "name": "mlp_offset"},
        ]

        self.optimizer = torch.optim.Adam(l, lr=0.0, eps=1e-15)
        self.xyz_scheduler_args = get_expon_lr_func(lr_init=training_args.position_lr_init*self.spatial_lr_scale,
                                                    lr_final=training_args.position_lr_final*self.spatial_lr_scale,
                                                    lr_delay_mult=training_args.position_lr_delay_mult,
                                                    max_steps=training_args.position_lr_max_steps)        
        
        self.mlp_opacity_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_opacity_lr_init,
                                                    lr_final=training_args.mlp_opacity_lr_final,
                                                    lr_delay_mult=training_args.mlp_opacity_lr_delay_mult,
                                                    max_steps=training_args.mlp_opacity_lr_max_steps)
        
        self.mlp_offset_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_offset_lr_init,
                                                    lr_final=training_args.mlp_offset_lr_final,
                                                    lr_delay_mult=training_args.mlp_offset_lr_delay_mult,
                                                    max_steps=training_args.mlp_offset_lr_max_steps)
        
        self.mlp_cholesky_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_cholesky_lr_init,
                                                    lr_final=training_args.mlp_cholesky_lr_final,
                                                    lr_delay_mult=training_args.mlp_cholesky_lr_delay_mult,
                                                    max_steps=training_args.mlp_cholesky_lr_max_steps)
        
        self.mlp_color_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_color_lr_init,
                                                    lr_final=training_args.mlp_color_lr_final,
                                                    lr_delay_mult=training_args.mlp_color_lr_delay_mult,
                                                    max_steps=training_args.mlp_color_lr_max_steps)

    def update_learning_rate(self, iteration):
        ''' Learning rate scheduling per step '''
        for param_group in self.optimizer.param_groups:
            if param_group["name"] == "xyz":
                lr = self.xyz_scheduler_args(iteration)
                param_group['lr'] = lr
            if param_group["name"] == "mlp_opacity":
                lr = self.mlp_opacity_scheduler_args(iteration)
                param_group['lr'] = lr
            if param_group["name"] == "mlp_cholesky":
                lr = self.mlp_cholesky_scheduler_args(iteration)
                param_group['lr'] = lr
            if param_group["name"] == "mlp_color":
                lr = self.mlp_color_scheduler_args(iteration)
                param_group['lr'] = lr
            if param_group["name"] == "mlp_offset":
                lr = self.mlp_offset_scheduler_args(iteration)
                param_group['lr'] = lr    
                
    def construct_list_of_attributes(self):
        l = ['x', 'y', 'z', 'nx', 'ny', 'nz']
        # All channels except the 3 DC
        for i in range(self._shs_dc.shape[1]*self._shs_dc.shape[2]):
            l.append('shs_dc_{}'.format(i))
        for i in range(self._shs_rest.shape[1]*self._shs_rest.shape[2]):
            l.append('shs_rest_{}'.format(i))
        l.append('opacity')
        for i in range(self._scaling.shape[1]):
            l.append('scale_{}'.format(i))
        for i in range(self._rotation.shape[1]):
            l.append('rot_{}'.format(i))
        for i in range(self._alpha.shape[1]):
            l.append('alpha_{}'.format(i))
        for i in range(self._offsets.shape[1]):
            l.append('offsets_{}'.format(i))
        for i in range(self._cholesky.shape[1]):
            l.append('chol_{}'.format(i))
        for i in range(self._feats.shape[1]):
            l.append('feat_{}'.format(i))
        return l

    def save_ply(self, path):
        mkdir_p(os.path.dirname(path))

        xyz = self._xyz.detach().cpu().numpy()  # (N, 3)
        normals = np.zeros_like(xyz)            # (N, 3)
        shs_dc = self._shs_dc.detach().transpose(1, 2).flatten(start_dim=1).contiguous().cpu().numpy()       # (N, 3)
        shs_rest = self._shs_rest.detach().transpose(1, 2).flatten(start_dim=1).contiguous().cpu().numpy()   # (N, 15*3)
        opacities = self._opacity.detach().cpu().numpy()    # (N, 1)
        scale = self._scaling.detach().cpu().numpy()        # (N, 3)
        rotation = self._rotation.detach().cpu().numpy()    # (N, 4)
        alpha = self._alpha.detach().cpu().numpy()          # (N, 1)
        offsets = self._offsets.detach().cpu().numpy()      # (N, 2)
        cholesky = self._cholesky.detach().cpu().numpy()    # (N, 3)
        feats = self._feats.detach().cpu().numpy()          # (N, feat_dim)

        dtype_full = [(attribute, 'f4') for attribute in self.construct_list_of_attributes()]

        elements = np.empty(xyz.shape[0], dtype=dtype_full)
        attributes = np.concatenate((xyz, normals, shs_dc, shs_rest, opacities, scale, rotation, alpha, offsets, cholesky, feats), axis=1)
        elements[:] = list(map(tuple, attributes))
        el = PlyElement.describe(elements, 'vertex')
        PlyData([el]).write(path)

    def reset_opacity(self):
        opacities_new = inverse_sigmoid(torch.min(self.get_opacity, torch.ones_like(self.get_opacity)*0.01))
        optimizable_tensors = self.replace_tensor_to_optimizer(opacities_new, "opacity")
        self._opacity = optimizable_tensors["opacity"]

    def load_ply(self, path):
        plydata = PlyData.read(path)

        xyz = np.stack((np.asarray(plydata.elements[0]["x"]),
                        np.asarray(plydata.elements[0]["y"]),
                        np.asarray(plydata.elements[0]["z"])),  axis=1)
        opacities = np.asarray(plydata.elements[0]["opacity"])[..., np.newaxis]

        shs_dc = np.zeros((xyz.shape[0], 3, 1))
        shs_dc[:, 0, 0] = np.asarray(plydata.elements[0]["shs_dc_0"])
        shs_dc[:, 1, 0] = np.asarray(plydata.elements[0]["shs_dc_1"])
        shs_dc[:, 2, 0] = np.asarray(plydata.elements[0]["shs_dc_2"])

        extra_shs_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("shs_rest_")]
        extra_shs_names = sorted(extra_shs_names, key = lambda x: int(x.split('_')[-1]))
        assert len(extra_shs_names)==3*(self.max_sh_degree + 1) ** 2 - 3
        shs_extra = np.zeros((xyz.shape[0], len(extra_shs_names)))
        for idx, attr_name in enumerate(extra_shs_names):
            shs_extra[:, idx] = np.asarray(plydata.elements[0][attr_name])
        # Reshape (P,F*SH_coeffs) to (P, F, SH_coeffs except DC)
        shs_extra = shs_extra.reshape((shs_extra.shape[0], 3, (self.max_sh_degree + 1) ** 2 - 1))

        scale_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("scale_")]
        scale_names = sorted(scale_names, key = lambda x: int(x.split('_')[-1]))
        scales = np.zeros((xyz.shape[0], len(scale_names)))
        for idx, attr_name in enumerate(scale_names):
            scales[:, idx] = np.asarray(plydata.elements[0][attr_name])

        rot_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("rot")]
        rot_names = sorted(rot_names, key = lambda x: int(x.split('_')[-1]))
        rots = np.zeros((xyz.shape[0], len(rot_names)))
        for idx, attr_name in enumerate(rot_names):
            rots[:, idx] = np.asarray(plydata.elements[0][attr_name])
        
        alpha_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("alpha")]
        alpha_names = sorted(alpha_names, key = lambda x: int(x.split('_')[-1]))
        alpha = np.zeros((xyz.shape[0], len(alpha_names)))
        for idx, attr_name in enumerate(alpha_names):
            alpha[:, idx] = np.asarray(plydata.elements[0][attr_name])
        
        offsets_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("offsets")]
        offsets_names = sorted(offsets_names, key = lambda x: int(x.split('_')[-1]))
        offsets = np.zeros((xyz.shape[0], len(offsets_names)))
        for idx, attr_name in enumerate(offsets_names):
            offsets[:, idx] = np.asarray(plydata.elements[0][attr_name])
        
        chol_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("chol")]
        chol_names = sorted(chol_names, key = lambda x: int(x.split('_')[-1]))
        cholesky = np.zeros((xyz.shape[0], len(chol_names)))
        for idx, attr_name in enumerate(chol_names):
            cholesky[:, idx] = np.asarray(plydata.elements[0][attr_name])
        
        feat_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("feat")]
        feat_names = sorted(feat_names, key = lambda x: int(x.split('_')[-1]))
        feats = np.zeros((xyz.shape[0], len(feat_names)))
        for idx, attr_name in enumerate(feat_names):
            feats[:, idx] = np.asarray(plydata.elements[0][attr_name])

        self._xyz = nn.Parameter(torch.tensor(xyz, dtype=torch.float, device="cuda").requires_grad_(True))
        self._shs_dc = nn.Parameter(torch.tensor(shs_dc, dtype=torch.float, device="cuda").transpose(1, 2).contiguous().requires_grad_(True))
        self._shs_rest = nn.Parameter(torch.tensor(shs_extra, dtype=torch.float, device="cuda").transpose(1, 2).contiguous().requires_grad_(True))
        self._opacity = nn.Parameter(torch.tensor(opacities, dtype=torch.float, device="cuda").requires_grad_(True))
        self._scaling = nn.Parameter(torch.tensor(scales, dtype=torch.float, device="cuda").requires_grad_(True))
        self._rotation = nn.Parameter(torch.tensor(rots, dtype=torch.float, device="cuda").requires_grad_(True))
        self._alpha = nn.Parameter(torch.tensor(alpha, dtype=torch.float, device="cuda").requires_grad_(True))
        self._offsets = nn.Parameter(torch.tensor(offsets, dtype=torch.float, device="cuda").requires_grad_(True))
        self._cholesky = nn.Parameter(torch.tensor(cholesky, dtype=torch.float, device="cuda").requires_grad_(True))
        self._feats = nn.Parameter(torch.tensor(feats, dtype=torch.float, device="cuda").requires_grad_(True))

        self.active_sh_degree = self.max_sh_degree

    def replace_tensor_to_optimizer(self, tensor, name):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            if group["name"] == name:
                stored_state = self.optimizer.state.get(group['params'][0], None)
                stored_state["exp_avg"] = torch.zeros_like(tensor)
                stored_state["exp_avg_sq"] = torch.zeros_like(tensor)

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(tensor.requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state

                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors

    def _prune_optimizer(self, mask):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:
                stored_state["exp_avg"] = stored_state["exp_avg"][mask]
                stored_state["exp_avg_sq"] = stored_state["exp_avg_sq"][mask]

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter((group["params"][0][mask].requires_grad_(True)))
                self.optimizer.state[group['params'][0]] = stored_state

                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(group["params"][0][mask].requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors

    def prune_points(self, mask):
        valid_points_mask = ~mask
        optimizable_tensors = self._prune_optimizer(valid_points_mask)

        self._xyz = optimizable_tensors["xyz"]
        self._shs_dc = optimizable_tensors["shs_dc"]
        self._shs_rest = optimizable_tensors["shs_rest"]
        self._opacity = optimizable_tensors["opacity"]
        self._scaling = optimizable_tensors["scaling"]
        self._rotation = optimizable_tensors["rotation"]
        self._alpha = optimizable_tensors["alpha"]
        self._offsets = optimizable_tensors["offsets"]
        self._cholesky = optimizable_tensors["cholesky"]
        self._feats = optimizable_tensors["feats"]

        self.xyz_gradient_accum = self.xyz_gradient_accum[valid_points_mask]

        self.denom = self.denom[valid_points_mask]
        self.max_radii2D = self.max_radii2D[valid_points_mask]

    def cat_tensors_to_optimizer(self, tensors_dict):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            assert len(group["params"]) == 1
            extension_tensor = tensors_dict[group["name"]]
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:

                stored_state["exp_avg"] = torch.cat((stored_state["exp_avg"], torch.zeros_like(extension_tensor)), dim=0)
                stored_state["exp_avg_sq"] = torch.cat((stored_state["exp_avg_sq"], torch.zeros_like(extension_tensor)), dim=0)

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state

                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]

        return optimizable_tensors
    
    def densification_postfix(self, new_xyz, new_shs_dc, new_shs_rest, new_opacities, new_scaling, new_rotation, new_alpha, new_offsets, new_cholesky, new_feats):
        d = {"xyz": new_xyz,
        "shs_dc": new_shs_dc,
        "shs_rest": new_shs_rest,
        "opacity": new_opacities,
        "scaling" : new_scaling,
        "rotation" : new_rotation,
        "alpha" : new_alpha,
        "offsets" : new_offsets,
        "cholesky" : new_cholesky,
        "feats": new_feats,
        }

        optimizable_tensors = self.cat_tensors_to_optimizer(d)
        self._xyz = optimizable_tensors["xyz"]              # (N+P, 3)
        self._shs_dc = optimizable_tensors["shs_dc"]     # (N+P, 1)
        self._shs_rest = optimizable_tensors["shs_rest"] # (N+P, 15)
        self._opacity = optimizable_tensors["opacity"]      # (N+P, 1)
        self._scaling = optimizable_tensors["scaling"]      # (N+P, 3)
        self._rotation = optimizable_tensors["rotation"]    # (N+P, 4)
        self._alpha = optimizable_tensors["alpha"]    # (N+P, 4)
        self._offsets = optimizable_tensors["offsets"]    # (N+P, 4)
        self._cholesky = optimizable_tensors["cholesky"]    # (N+P, 4)
        self._feats = optimizable_tensors["feats"]    # (N+P, 4)

        self.xyz_gradient_accum = torch.zeros((self.get_xyz.shape[0], 1), device="cuda")
        self.denom = torch.zeros((self.get_xyz.shape[0], 1), device="cuda")
        self.max_radii2D = torch.zeros((self.get_xyz.shape[0]), device="cuda")
 
    def densify_and_split(self, grads, grad_threshold, scene_extent, N=2):
        n_init_points = self.get_xyz.shape[0]
        padded_grad = torch.zeros((n_init_points), device="cuda")
        padded_grad[:grads.shape[0]] = grads.squeeze()
        selected_pts_mask = torch.where(padded_grad >= grad_threshold, True, False)
        selected_pts_mask = torch.logical_and(selected_pts_mask,
                                              torch.max(self.get_scaling, dim=1).values > self.percent_dense*scene_extent)        
        stds = self.get_scaling[selected_pts_mask].repeat(N,1)  # (P, 3)
        means = torch.zeros((stds.size(0), 3),device="cuda")    # (P, 3)
        samples = torch.normal(mean=means, std=stds)            # (P, 3)
        rots = build_rotation(self._rotation[selected_pts_mask]).repeat(N,1,1)
        new_xyz = torch.bmm(rots, samples.unsqueeze(-1)).squeeze(-1) + self.get_xyz[selected_pts_mask].repeat(N, 1)
        new_scaling = self.scaling_inverse_activation(self.get_scaling[selected_pts_mask].repeat(N,1) / (0.8*N))
        new_rotation = self._rotation[selected_pts_mask].repeat(N,1)
        new_shs_dc = self._shs_dc[selected_pts_mask].repeat(N,1,1)
        new_shs_rest = self._shs_rest[selected_pts_mask].repeat(N,1,1)
        new_opacity = self._opacity[selected_pts_mask].repeat(N,1)
        new_alpha = self._alpha[selected_pts_mask].repeat(N,1)
        new_offsets = self._offsets[selected_pts_mask].repeat(N,1)
        new_cholesky = self._cholesky[selected_pts_mask].repeat(N,1)
        new_feats = self._feats[selected_pts_mask].repeat(N,1)

        self.densification_postfix(new_xyz, new_shs_dc, new_shs_rest, new_opacity, new_scaling, new_rotation, new_alpha, new_offsets, new_cholesky, new_feats)

        prune_filter = torch.cat((selected_pts_mask, torch.zeros(N * selected_pts_mask.sum(), device="cuda", dtype=bool)))
        self.prune_points(prune_filter)

    def densify_and_clone(self, grads, grad_threshold, scene_extent):
        selected_pts_mask = torch.where(torch.norm(grads, dim=-1) >= grad_threshold, True, False)
        selected_pts_mask = torch.logical_and(selected_pts_mask,
                                              torch.max(self.get_scaling, dim=1).values <= self.percent_dense*scene_extent)
        
        new_xyz = self._xyz[selected_pts_mask]                      # (P, 3)
        new_shs_dc = self._shs_dc[selected_pts_mask]      # (P, 1)
        new_shs_rest = self._shs_rest[selected_pts_mask]  # (P, 15)
        new_opacities = self._opacity[selected_pts_mask]  # (P, 1)
        new_scaling = self._scaling[selected_pts_mask]    # (P, 3)
        new_rotation = self._rotation[selected_pts_mask]  # (P, 4)
        new_alpha = self._alpha[selected_pts_mask]        # (P, 1)
        new_offsets = self._offsets[selected_pts_mask]    # (P, 2)
        new_cholesky = self._cholesky[selected_pts_mask]  # (P, 3)
        new_feats = self._feats[selected_pts_mask]  # (P, 3)

        self.densification_postfix(new_xyz, new_shs_dc, new_shs_rest, new_opacities, new_scaling, new_rotation, new_alpha, new_offsets, new_cholesky, new_feats)

    def densify_and_prune(self, max_grad, min_opacity, extent, max_screen_size):
        grads = self.xyz_gradient_accum / self.denom   
        grads[grads.isnan()] = 0.0 

        self.densify_and_clone(grads, max_grad, extent)
        self.densify_and_split(grads, max_grad, extent)

        prune_mask = (self.get_opacity < min_opacity).squeeze()
        if max_screen_size:
            big_points_vs = self.max_radii2D > max_screen_size
            big_points_ws = self.get_scaling.max(dim=1).values > 0.1 * extent
            prune_mask = torch.logical_or(torch.logical_or(prune_mask, big_points_vs), big_points_ws)
        self.prune_points(prune_mask)

        torch.cuda.empty_cache()

    def add_densification_stats(self, viewspace_point_tensor, update_filter):
        self.xyz_gradient_accum[update_filter] += torch.norm(viewspace_point_tensor.grad[update_filter,:2], dim=-1, keepdim=True)
        self.denom[update_filter] += 1
        
    def save_checkpoints(self, path):
        mkdir_p(os.path.dirname(path))
        ckpt = {
            'opacity_mlp': self.mlp_opacity.state_dict(),
            'cholesky_mlp': self.mlp_cholesky.state_dict(),
            'color_mlp': self.mlp_color.state_dict(),
            'offset_mlp': self.mlp_offset.state_dict(),
            }
        torch.save(ckpt, path)


    def load_checkpoints(self, path):
        checkpoint = torch.load(path)
        try:
            self.mlp_opacity.load_state_dict(checkpoint['opacity_mlp'])
            self.mlp_cholesky.load_state_dict(checkpoint['cholesky_mlp'])
            self.mlp_color.load_state_dict(checkpoint['color_mlp'])
            self.mlp_offset.load_state_dict(checkpoint['offset_mlp'])
        except:
            print(f"Loading checkpoints from {path} un successfully !")
    